#include "continuous_linear_system.h"
#include <iostream>
#include "matplotlibcpp.hpp"
#include "mpc.h"
#include <random>
#include <deque>
using std::default_random_engine;
using std::uniform_real_distribution;
using namespace LinearMpc;
namespace plt = matplotlibcpp;
int main()
{
	//define system
	//ContinuousLinearSys(uint state_dim, uint control_dim, uint distur_dim = 0);
	unsigned int state_dim = 1;
	unsigned int control_dim = 1;
	unsigned int distur_dim = 1;
	ContinuousLinearSys longti_sys(state_dim, control_dim, distur_dim);
	Eigen::MatrixXd state_matrix(state_dim, state_dim);
	state_matrix << 0;
	Eigen::MatrixXd control_matrix(state_dim, control_dim);
	control_matrix << 1;
	Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
	distur_matrix << 1;
	longti_sys.set_matrix(state_matrix, control_matrix, distur_matrix);
	std::cout << "init state_matrix is:" << std::endl
			  << longti_sys.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << longti_sys.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << longti_sys.distur_matrix() << std::endl;
	//	std::shared_ptr<DiscreteLinearSys> discretization(
	//	double delta_time, std::string method = "eular", double exact_torrence = 1.0e-9);
	double rate = 100;
	std::shared_ptr<DiscreteLinearSys> dis_sys = longti_sys.discretization(1 / rate, "tustin");
	//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
	//unsigned int max_xu_constraint, unsigned int max_delta_x, unsigned int max_delta_u, unsigned int max_final_constraint);
	std::cout << "dis_sys state_matrix is:" << std::endl
			  << dis_sys->state_matrix() << std::endl;
	std::cout << "dis_sys control matrix is:" << std::endl
			  << dis_sys->control_matrix() << std::endl;
	std::cout << "dis_sys distur matrix is:" << std::endl
			  << dis_sys->distur_matrix() << std::endl;
	unsigned int mpc_windows = 100;
	Mpc longti_mpc(dis_sys, mpc_windows, 1, 0, 1, 0);
	/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
					   const std::vector<double> u_index, const std::vector<double> u_factor,
					   int constraint_index, double min, double max)*/
	longti_mpc.set_xu_affine({}, {}, {0}, {1}, 0, -5, 5);
	//set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max)
	longti_mpc.set_delta_u(0, 0, -0.05, 0.05);
	//longti_mpc.set_xu_affine({0}, {1}, {}, {}, 1, -5, 5);
	//longti_mpc.set_xu_affine({}, {}, {0}, {1}, 0, -1000, 1000);
	//longti_mpc.set_xu_affine({0}, {1.0}, {}, {}, 0, -5.0, 5.0);
	/*int Mpc::set_final_constraint(int constraint_index, const std::vector<double> x_index,
							  const std::vector<double> x_factor, double min, double max)*/
	//longti_mpc.set_final_constraint(0, {0}, {1}, 1000.0, 1000.0);
	/*bool Mpc::update_mpc(
	const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
	const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref,
	const Eigen::MatrixXd &distur_esti)*/
	Eigen::MatrixXd q(state_dim, state_dim);
	q << 1.0;
	Eigen::MatrixXd r(control_dim, control_dim);
	r << 0.0;
	Eigen::VectorXd x0(state_dim);
	x0 << 0;
	Eigen::VectorXd u_fb(control_dim);
	u_fb << 0.0;
	Eigen::MatrixXd x_ref(state_dim, 1);
	x_ref << 0.5;
	Eigen::MatrixXd distur_esti(distur_dim, 1);
	distur_esti << -1;
	Eigen::MatrixXd distur_actual(distur_dim, 1);
	distur_actual << -1.2;
	/*std::cout << "hessian matrix is:" << std::endl;
	std::cout << Eigen::MatrixXd(longti_mpc.hessian()) << std::endl;
	std::cout << "gradient is:" << std::endl;
	std::cout << longti_mpc.gradient() << std::endl;
	std::cout << "constriants is:" << std::endl;
	std::cout << Eigen::MatrixXd(longti_mpc.constraint_matrix()) << std::endl;
	std::cout << "bound is:" << std::endl;
	std::cout << longti_mpc.upperBound().transpose() << std::endl;
	std::cout << longti_mpc.lowerBound().transpose() << std::endl;*/

	int max_iter = 1000;
	std::vector<double> acc_vector;
	std::vector<double> speed_vector;
	std::vector<double> speed_error_vector;
	unsigned int filter_windows = 50;
	std::deque<double> acc_deque(filter_windows, -1.2);
	double acc_distur_esti = 0;
	double speed_error = 0;
	default_random_engine e;
	uniform_real_distribution<double> rand_var(-0.1, 0.1);
	for (int i = 0; i < max_iter; i++)
	{

		acc_deque.pop_front();
		acc_deque.push_back(speed_error * rate);
		double acc_error_sum = 0;
		for (auto it = acc_deque.begin(); it != acc_deque.end(); ++it)
		{
			//acc_error_sum += *it;
			acc_error_sum += distur_actual(0) - 0.5 + rand_var(e);
		}
		acc_distur_esti = acc_error_sum / filter_windows;
		distur_esti << acc_distur_esti; //acc_distur_esti;
		if (!longti_mpc.update_mpc(q, r, x0, u_fb, x_ref, distur_esti))
		{
			std::cout << "\033[31m mpc update failed \033[0m" << std::endl;
		}
		longti_mpc.mpc_solve();
		u_fb = longti_mpc.get_contorl_one_shot(0);
		std::cout << i << " u is:" << u_fb << std::endl;
		/*ContinuousLinearSys::integral(
	const uint segment, const double start_time, const double end_time,
	const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd xf) */
		Eigen::VectorXd xf_real(2);
		longti_sys.integral(1, 0, 1 / rate, x0, u_fb, distur_actual, xf_real);
		x0 = xf_real;
		Eigen::VectorXd xf_model(2);
		Eigen::MatrixXd zero_distur(1, 1);
		zero_distur << 0;
		longti_sys.integral(1, 0, 1 / rate, x0, u_fb, zero_distur, xf_model);
		speed_error = xf_real(0) - xf_model(0);
		acc_vector.push_back(u_fb(0));
		speed_vector.push_back(xf_real(0));
		speed_error_vector.push_back(acc_distur_esti);
	}
	plt::named_plot("speed", speed_vector);
	plt::legend();

	plt::named_plot("acc", acc_vector);
	plt::legend();

	plt::named_plot("speed_error", speed_error_vector);
	plt::legend();
	plt::show();

	/*auto jerk_vector = longti_mpc.get_control_vector(0);
	plt::plot(jerk_vector);
	plt::show();
	auto acc_vector = longti_mpc.get_state_vector(0);
	plt::plot(acc_vector);
	plt::show();
	auto speed_vector = longti_mpc.get_state_vector(1);
	plt::plot(speed_vector);
	plt::show();

	/*t::plot(jerk_vector);
	plt::show();
	plt::plot(acc_vector);
	plt::show();
	plt::plot(speed_vector);
	plt::show();*/
}